﻿using System;
using System.Collections.Generic;
using System.Threading;
using UnityEditor;
using UnityEngine;
using UnityEngine.Rendering;
using Object = System.Object;

[RequireComponent(typeof(MeshFilter)), RequireComponent(typeof(MeshRenderer))]
public class NewBehaviourScript : MonoBehaviour
{
    [SerializeField] public string[] grammars;

    [SerializeField] private string axiom = "";

    [SerializeField, Range(0, 10)] private int step = 2;

    [SerializeField, TextArea(5, 5)] private string sentence = "";

    [SerializeField, Range(0.5f, 3f)] private float len = 10;

    [SerializeField, Range(5, 60)] private float angle = 25;

    [SerializeField, Range(0.5f, 5f)] private float radius = 1f;

    [SerializeField, Range(0.01f, 0.5f)]private float growthRings = 0.03f;
    
    [SerializeField, Range(0.05f, 1f)] private float leafSize = 0.5f;

    [SerializeField, Range(0.1f, 1f)] private float lenFactor = 0.85f;

    [SerializeField, Range(0.1f, 1f)] private float radiusFactor = 0.85f;

    [Tooltip("必须可以整除360")] [SerializeField] private int vertNum = 5;
    [Tooltip("控制点的树木")] public int num = 0;
    [Tooltip("模型顶点数")] public int verticesNum = 0;

    [Header("Debug")] [Tooltip("显示控制点")] public bool showPoint = false;
    [Tooltip("显示顶点本地坐标系")] public bool showCoord = false;
    [Tooltip("显示骨架")] public bool showSkeleton = false;
    [Tooltip("显示网格顶点")] public bool showMeshVextices = false;
    [Tooltip("显示网格")] public bool showMesh = false;
    [Tooltip("模型的材质")]public Material material;

    private Mesh _mesh;
    private MeshFilter meshFilter;
    private MeshRenderer meshRenderer;

    struct Point
    {
        public Vector3 direction;
        public Vector3 position;
        public Quaternion rotation;
        public float r;
        public float len;
        public int index;
        public int parent;
        public int childNo; // 索引从1开始!
        public int[] children;
        public int[][] vertices;
        public bool isBranchPoint;
        public bool isLeaf;
        public float uvHandle;
        
        public BasisCoord basis;
    }
    
    struct Leaf
    {
        public Vector3[] vertices;
        public int[] vertIndices;
        public BasisCoord basis;
    }

    public struct BasisCoord
    {
        public Vector3 x;
        public Vector3 y;
        public Vector3 z;
    }

    // 存储数组
    private Point[] points;
    private int p = 0;
    private Point[] stack;
    private int top = -1;

    private Point lastPoint;


    // BFS遍历队列
    private Queue<Point> queue;


    // 树干
    private List<Vector3> normals;
    private List<Vector4> tangents;
    private List<Vector3> vertices;
    private List<int> triangles;
    private List<Vector2> uvs;
    private int v = 0;
    private int tri = 0;
    
    // 树叶
    private List<Leaf> leafs; 
    private List<int> leafTriangles;


    private void OnEnable()
    {
        meshFilter = GetComponent<MeshFilter>();
        meshRenderer = GetComponent<MeshRenderer>();
        if (!showMesh)
        {
            GetComponent<MeshFilter>().mesh = null;
            GetComponent<MeshRenderer>().material = null;
        }
        
        ClearAll();
        
        var lSystemManager = new LSystemManager();
        for (int i = 0; grammars!= null && i < grammars.Length; i++) {
            lSystemManager.AddGrammar(grammars[i]);
        }
        // 生成L-System文法
        sentence = lSystemManager.GenerateSentence(axiom,step, ref num); 

        // 初始化列表
        points = new Point[num + 1];
        stack = new Point[num + 1];
        vertices = new List<Vector3>();
        uvs = new List<Vector2>();
        normals = new List<Vector3>();
        tangents = new List<Vector4>();
        triangles = new List<int>();
        queue = new Queue<Point>();
        leafs = new List<Leaf>();
        leafTriangles = new List<int>();

        CreateTree();
        
        if (showMesh) {
            meshFilter.mesh = _mesh = new Mesh();
            _mesh.indexFormat = IndexFormat.UInt32;
            _mesh.subMeshCount = 2;
            _mesh.vertices = vertices.ToArray();
            _mesh.normals = normals.ToArray();
            _mesh.tangents = tangents.ToArray();
            _mesh.uv = uvs.ToArray();
            _mesh.SetTriangles(triangles,0);
            _mesh.SetTriangles(leafTriangles,1);
            _mesh.RecalculateNormals();
            _mesh.RecalculateTangents();
            _mesh.RecalculateBounds();
        }
    }
    

    private void CreateTree()
    {
        //生成树的骨架
        GenerateFramework();
        // 构建网格
        queue.Enqueue(points[0]);
        while (queue.Count > 0) {
            Point current = queue.Dequeue();
            // 如果叶子节点
            if (points[current.index].isLeaf) {
                CreateMeshVertices(0, current.index);
                int rings = (int) (points[current.index].r / growthRings);
                for (int i = 1; i < rings; i++) {
                    CreateMeshVertices(i, current.index);
                }
            }
            else {
                // 构造出分叉结构
                if (points[current.index].isBranchPoint) {
                    // 遍历孩子节点
                    for (int i = 1; i <= current.children[0]; i++) {
                        int childIndex = current.children[i];
                        queue.Enqueue(points[childIndex]);
                        // 为参分支节点以分叉后点多个节点作为参考方向
                        CreateMeshVertices(i - 1, current.index, childIndex);
                    }

                    CreateMeshVertices(current.children[0], current.index);
                }
                else {
                    queue.Enqueue(points[current.children[1]]);
                    CreateMeshVertices(0, current.index);
                }
            }

            // 开始连接定点索引
            // 如果不是树根顶点
            if (points[current.index].index != points[current.index].parent) {
                // 如果不是叶子节点
                if (true) {
                    // 上方顶点
                    int[] indices1;
                    // 如果现在的顶点是分叉点
                    if (points[current.index].isBranchPoint) {
                        indices1 = points[current.index].vertices[points[current.index].vertices.Length - 1];
                    }
                    else {
                        indices1 = points[current.index].vertices[0];
                    }

                    int vertIndex = points[current.index].childNo - 1; // 索引从1开始
                    int parent = points[current.index].parent;
                    // 下方顶点
                    int[] indices2 = points[parent].vertices[vertIndex];

                    int iter = 0;
                    int idx = 0;
                    while (iter < vertNum + 1) {
                        triangles.Add(indices2[idx]);
                        triangles.Add(indices2[(idx + 1) % (vertNum + 1)]);
                        triangles.Add(indices1[idx]);
                        triangles.Add(indices2[(idx + 1) % (vertNum + 1)]);
                        triangles.Add(indices1[(idx + 1) % (vertNum + 1)]);
                        triangles.Add(indices1[idx]);
                        // triangles[tri] = indices2[idx];
                        // triangles[tri + 3] = triangles[tri + 1] = indices2[(idx + 1) % vertNum];
                        // triangles[tri + 5] = triangles[tri + 2] = indices1[idx];
                        // triangles[tri + 4] = indices1[(idx + 1) % vertNum];
                        tri += 6;
                        idx += 1;
                        iter++;
                    }
                }

                if (points[current.index].isLeaf) {
                    int iter = 0;
                    int rings = Mathf.Max(1, (int) (points[current.index].r / growthRings));
                    for (int i = 0; i < rings - 1; i++) {
                        int[] indices2 = points[current.index].vertices[i];
                        int[] indices1 = points[current.index].vertices[i + 1];
                        iter = 0;
                        while (iter < vertNum) {
                            triangles.Add(indices2[iter]);
                            triangles.Add(indices2[(iter + 1) % (vertNum + 1)]);
                            triangles.Add(indices1[iter]);
                            triangles.Add(indices2[(iter + 1) % (vertNum + 1)]);
                            triangles.Add(indices1[(iter + 1) % (vertNum + 1)]);
                            triangles.Add(indices1[iter]);
                            tri += 6;
                            iter++;
                        }
                    }

                    int[] indices = points[current.index].vertices[rings - 1];
                    int index = points[current.index].vertices[rings][0];
                    iter = 0;
                    while (iter < vertNum) {
                        triangles.Add(indices[iter]);
                        triangles.Add(indices[(iter + 1) % (vertNum + 1)]);
                        triangles.Add(index);
                        tri += 3;
                        iter++;
                    }
                }
            }
        }
        
        verticesNum = vertices.Count;
    }

    private void CreateGrowthRingVertices(int vertIndex,int currentIndex,float r)
    {
        // 获取基变换矩阵
        Matrix4x4 basis = MakeBasis(
            points[currentIndex].basis.x,
            points[currentIndex].basis.y,
            points[currentIndex].basis.z);
        int iter = 0;
        int stride = (int)Mathf.Ceil(360f / vertNum);
        for (int deg = 0; deg < 360 + stride; deg += stride)
        {
            if (deg > 360) {
                deg = 360;
            }
            float rad = Mathf.Deg2Rad * deg;
            Vector3 circle = new Vector3(
                Mathf.Cos(rad) * r,
                Mathf.Sin(rad) * r,
                0.0f);
            Vector3 targetPos = points[currentIndex].position + basis.MultiplyPoint3x4(circle);

            // 构造每个控制点的网格顶点的索引
            points[currentIndex].vertices[vertIndex][iter] = v;
            if (vertIndex > 0) {
                points[currentIndex].uvHandle *= -1;
            }
            //float u = -Math.Abs(2.0f * iter / vertNum - 1) + 1;
            uvs.Add(new Vector2(1f * iter / vertNum, (points[currentIndex].uvHandle + 1f) / 2f));
            normals.Add(Vector3.Normalize(targetPos - points[currentIndex].position));
            tangents.Add(new Vector4(
                points[currentIndex].direction.x,
                points[currentIndex].direction.y,
                points[currentIndex].direction.z,
                -1f));
            vertices.Add(targetPos);
            v++;
            iter++;
        }
    }
    
    private void CreateMeshVertices(int vertIndex, int currentIndex)
    {
        // // 如果是叶子节点
        // if (points[currentIndex].isLeaf)
        // {
        //     points[currentIndex].vertices = new int[1][];
        //     points[currentIndex].vertices[0] = new int[1];
        //     points[currentIndex].vertices[0][0] = v;
        //     uvs.Add(new Vector2(0, points[currentIndex].uvHandle));
        //     normals.Add(points[currentIndex].direction);
        //     tangents.Add(new Vector4(0, 0, 0, -1f));
        //     vertices.Add(points[currentIndex].position);
        //     v++;
        //     return;
        // }
        
        // 如果是叶子节点
        if (points[currentIndex].isLeaf)
        {
            int rings = Mathf.Max(1,(int)(points[currentIndex].r / growthRings));
            if (points[currentIndex].vertices == null) {
                points[currentIndex].vertices = new int[rings + 1][];
                for (int i = 0; i < rings; i++)
                {
                    points[currentIndex].vertices[i] = new int[vertNum + 1];
                }
                points[currentIndex].vertices[rings] = new int[1];
            }

            for (int i = 0; i < rings; i++) {
                float r = points[currentIndex].r - i * growthRings;
                CreateGrowthRingVertices(i, currentIndex, r);
            }
            points[currentIndex].vertices[rings][0] = v;
            uvs.Add(new Vector2(0, points[currentIndex].uvHandle * Mathf.Pow(-1,rings-1) * -1));
            normals.Add(points[currentIndex].direction);
            tangents.Add(new Vector4(0, 0, 0, -1f));
            vertices.Add(points[currentIndex].position);
            v++;
            return;
        }
        

        if (points[currentIndex].vertices == null)
        {
            // 分叉节点需要多存储一个网格顶点索引数组
            if (points[currentIndex].isBranchPoint)
            {
                int childCount = points[currentIndex].children[0];
                points[currentIndex].vertices = new int[childCount + 1][];
                for (int i = 0; i < childCount + 1; i++)
                {
                    points[currentIndex].vertices[i] = new int[vertNum + 1];
                }
            }
            // 一般节点
            else
            {
                points[currentIndex].vertices = new int[1][];
                points[currentIndex].vertices[0] = new int[vertNum + 1];
            }
        }

        // 获取基变换矩阵
        Matrix4x4 basis = MakeBasis(
            points[currentIndex].basis.x,
            points[currentIndex].basis.y,
            points[currentIndex].basis.z);
        int iter = 0;
        int stride = (int)Mathf.Ceil(360f / vertNum);
        for (int deg = 0; deg < 360 + stride; deg += stride)
        {
            if (deg > 360) {
                deg = 360;
            }
            float rad = Mathf.Deg2Rad * deg;
            Vector3 circle = new Vector3(
                Mathf.Cos(rad) * points[currentIndex].r,
                Mathf.Sin(rad) * points[currentIndex].r,
                0.0f);
            Vector3 targetPos = points[currentIndex].position + basis.MultiplyPoint3x4(circle);

            // 构造每个控制点的网格顶点的索引
            points[currentIndex].vertices[vertIndex][iter] = v;
            //float u = -Math.Abs(2.0f * iter / vertNum - 1) + 1;
            uvs.Add(new Vector2(1f * iter / vertNum, (points[currentIndex].uvHandle + 1f) / 2f));
            normals.Add(Vector3.Normalize(targetPos - points[currentIndex].position));
            tangents.Add(new Vector4(
                points[currentIndex].direction.x,
                points[currentIndex].direction.y,
                points[currentIndex].direction.z,
                -1f));
            vertices.Add(targetPos);
            v++;
            iter++;
        }
    }

    private void CreateMeshVertices(int vertIndex, int currentIndex, int refPointIndex)
    {
        // // 如果是叶子节点
        // if (points[currentIndex].isLeaf)
        // {
        //     points[currentIndex].vertices = new int[1][];
        //     points[currentIndex].vertices[0] = new int[1];
        //     points[currentIndex].vertices[0][0] = v;
        //     uvs.Add(new Vector2(0, 1));
        //     normals.Add(points[currentIndex].direction);
        //     tangents.Add(new Vector4(0, 0, 0, -1f));
        //     vertices.Add(points[currentIndex].position);
        //     v++;
        //     return;
        // }

        if (points[currentIndex].vertices == null)
        {
            // 分叉节点需要多存储一个网格顶点索引数组
            if (points[currentIndex].isBranchPoint)
            {
                int childCount = points[currentIndex].children[0];
                points[currentIndex].vertices = new int[childCount + 1][];
                for (int i = 0; i < childCount + 1; i++)
                {
                    points[currentIndex].vertices[i] = new int[vertNum + 1];
                }
            }
            // 一般节点
            else
            {
                points[currentIndex].vertices = new int[1][];
                points[currentIndex].vertices[0] = new int[vertNum + 1];
            }
        }

        // 获取基变换矩阵
        Matrix4x4 basis = MakeBasis(
            points[refPointIndex].basis.x,
            points[refPointIndex].basis.y,
            points[refPointIndex].basis.z);
        int iter = 0;
        int stride = (int)Mathf.Ceil(360f / vertNum);
        for (int deg = 0; deg < 360 + stride; deg += stride)
        {
            if (deg >= 360) {
                deg = 360;
            }
            float rad = Mathf.Deg2Rad * deg;
            Vector3 circle = new Vector3(
                Mathf.Cos(rad) * points[currentIndex].r,
                Mathf.Sin(rad) * points[currentIndex].r,
                0.0f);
            Vector3 targetPos = points[currentIndex].position + basis.MultiplyPoint3x4(circle);

            // 构造每个控制点的网格顶点的索引
            points[currentIndex].vertices[vertIndex][iter] = v;
            //float u = -Math.Abs(2.0f * iter / vertNum - 1) + 1;
            uvs.Add(new Vector2(1f * iter / vertNum, (points[currentIndex].uvHandle + 1f) / 2f));

            normals.Add(Vector3.Normalize(targetPos - points[currentIndex].position));
            tangents.Add(new Vector4(
                points[refPointIndex].direction.x,
                points[refPointIndex].direction.y,
                points[refPointIndex].direction.z,
                -1f));
            vertices.Add(targetPos);
            v++;
            iter++;
        }
    }

    private void GenerateFramework()
    {
        // 初始化根节点S
        Point root = new Point
        {
            direction = Vector3.up,
            position = Vector3.zero,
            rotation = Quaternion.identity,
            r = radius,
            len = len,
            index = p,
            isLeaf = true,
            isBranchPoint = false,
            uvHandle = 1f,
        };
        // 基变换
        Vector3 direction = Vector3.Normalize(root.direction);
        Vector3 right = Vector3.Cross(Vector3.up, direction).normalized;
        if (right == Vector3.zero)
        {
            right = Vector3.right;
        }

        Vector3 up = Vector3.Cross(direction, right);
        BasisCoord basis = new BasisCoord {x = right, y = up, z = direction};
        root.basis = basis;
        points[p++] = root;
        lastPoint = root;

        for (int i = 0; i < sentence.Length; i++)
        {
            char c = sentence[i];
            Quaternion rot;
            switch (c)
            {
                case 'F':
                    CreatePoint();
                    num++;
                    break;
                case 'X':
                    CreateLeaf();
                    break;
                case '+':
                    rot = Quaternion.Euler(0, 0, angle);
                    RotationPoint(rot);
                    break;
                case '-':
                    rot = Quaternion.Euler(0, 0, -angle);
                    RotationPoint(rot);
                    break;
                case '*':
                    rot = Quaternion.Euler(0, -angle, 0);
                    RotationPoint(rot);
                    break;
                case '/':
                    rot = Quaternion.Euler(0, angle, 0);
                    RotationPoint(rot);
                    break;
                case '^':
                    rot = Quaternion.Euler(-angle, 0, 0);
                    RotationPoint(rot);
                    break;
                case '&':
                    rot = Quaternion.Euler(angle, 0, 0);
                    RotationPoint(rot);
                    break;
                case '[':
                    PushPoint();
                    break;
                case ']':
                    lastPoint = PopPoint();
                    break;
                default:
                    break;
            }
        }
    }

    private void CreateLeaf()
    {
        Leaf leaf = new Leaf();
        leaf.vertices = new Vector3[4];
        leaf.vertIndices = new int[4];
        leaf.basis = lastPoint.basis;
        Matrix4x4 basis = MakeBasis(lastPoint.basis.x, lastPoint.basis.y, lastPoint.basis.z);

        Vector3[] quad = new Vector3[4];
        quad[0] = new Vector3(leafSize,leafSize,0);
        quad[1] = new Vector3(leafSize,-leafSize,0);
        quad[2] = new Vector3(-leafSize,-leafSize,0);
        quad[3] = new Vector3(-leafSize,leafSize,0);
        for (int i = 0; i < quad.Length; i++) {
            Vector3 tarPos = lastPoint.position + basis.MultiplyPoint3x4(quad[i]); 
            leaf.vertices[i] = tarPos;
        }
        
        for (int i = 0; i < 4; i++) {
            leaf.vertIndices[i] = v;
            vertices.Add(leaf.vertices[i]);
            normals.Add(leaf.basis.z);
            tangents.Add(new Vector4(
                leaf.basis.x.x,
                leaf.basis.x.y,
                leaf.basis.x.z,-1));
            v++;
        }
        leafTriangles.Add(leaf.vertIndices[0]);
        leafTriangles.Add(leaf.vertIndices[2]);
        leafTriangles.Add(leaf.vertIndices[1]);
        leafTriangles.Add(leaf.vertIndices[0]);
        leafTriangles.Add(leaf.vertIndices[3]);
        leafTriangles.Add(leaf.vertIndices[2]);
        tri += 6;
        uvs.Add(new Vector2(1,1));
        uvs.Add(new Vector2(1,0));
        uvs.Add(new Vector2(0,0));
        uvs.Add(new Vector2(0,1));
        leafs.Add(leaf);
    }
    
    private void RotationPoint(Quaternion rot)
    {
        lastPoint.rotation *= rot;
        lastPoint.rotation = Quaternion.Normalize(lastPoint.rotation);
        lastPoint.direction = Vector3.Normalize(rot * lastPoint.direction);
        lastPoint.basis.x = rot * lastPoint.basis.x;
        lastPoint.basis.y = rot * lastPoint.basis.y;
        lastPoint.basis.z = rot * lastPoint.basis.z;
    }

    private void CreatePoint()
    {
        // 生成新的节点
        Point currentPoint = new Point();
        // 继承半径
        currentPoint.len = Mathf.Max(0.05f * len,lastPoint.len * lenFactor);
        // 继承半径
        currentPoint.r = Mathf.Max(0.1f * radius,lastPoint.r * radiusFactor);
        
        // 进行平移（已经进行了旋转）
        currentPoint.position = lastPoint.position + lastPoint.direction * (currentPoint.len + currentPoint.r + lastPoint.r);
        // 继承朝向
        currentPoint.direction = lastPoint.direction;
        // 继承旋转
        currentPoint.rotation = lastPoint.rotation;
       
        // 标记存储位置
        currentPoint.index = p;
        // 设置uv控制器
        currentPoint.uvHandle = lastPoint.uvHandle * -1f;
        // 记录父节点
        currentPoint.parent = lastPoint.index;
        // 记录基坐标变换
        currentPoint.basis = lastPoint.basis;
        // 默认都是叶节点
        currentPoint.isLeaf = true;


        // 父节点添加孩子
        if (points[lastPoint.index].children == null)
        {
            points[lastPoint.index].children = new int[500];
            points[lastPoint.index].children[0] = 0;
            // 断定不是叶子结点
            points[lastPoint.index].isLeaf = false;
        }

        // 记录每个父节点的孩子信息，持续更新状态
        points[lastPoint.index].children[++points[lastPoint.index].children[0]] = currentPoint.index;
        currentPoint.childNo = points[lastPoint.index].children[0];
        if (points[lastPoint.index].children[0] > 1)
        {
            points[lastPoint.index].isBranchPoint = true;
        }

        // 持久化节点信息
        points[p++] = currentPoint;

        lastPoint = currentPoint;
    }

    private void OnDisable()
    {
        ClearAll();
    }
    
    private void OnValidate()
    {
        OnDisable();
        OnEnable();
    }

    private void OnDrawGizmos()
    {
        for (int i = 0; i < p; i++)
        {
            // 绘制控制点
            if (showPoint)
            {
                Gizmos.color = Color.red;
                if (points[i].isBranchPoint)
                {
                    Gizmos.color = Color.magenta;
                }
                else if (points[i].isLeaf)
                {
                    Gizmos.color = Color.white;
                }

                Gizmos.DrawSphere(transform.TransformPoint(points[i].position), points[i].r);
            }

            if (showMeshVextices)
            {
                // 每个控制点的网格顶点索引数组
                int len = points[i].vertices.Length;

                for (int j = 0; j < len; j++)
                {
                    int[] indices = points[i].vertices[j];

                    for (int k = 0; k < indices.Length; k++)
                    {
                        Gizmos.color = Color.Lerp(Color.blue, Color.red, 1.0f * (k % vertNum) / (vertNum - 1));
                        Gizmos.DrawSphere(transform.TransformPoint(vertices[indices[k]]), 0.08f * points[i].r);
                    }
                }
            }

            if (showSkeleton)
            {
                Gizmos.color = Color.black;
                Gizmos.DrawLine(transform.TransformPoint(points[points[i].parent].position),
                    transform.TransformPoint(points[i].position));
            }

            if (showCoord)
            {
                Gizmos.color = Color.blue;
                Gizmos.DrawRay(transform.TransformPoint(points[i].position), points[i].basis.z);
                Gizmos.color = Color.red;
                Gizmos.DrawRay(transform.TransformPoint(points[i].position), points[i].basis.x);
                Gizmos.color = Color.green;
                Gizmos.DrawRay(transform.TransformPoint(points[i].position), points[i].basis.y);
            }
            
        }
    }


    // Update is called once per frame
    void Update()
    {
        if (!showMesh)
        {
            GetComponent<MeshFilter>().mesh = null;
            GetComponent<MeshRenderer>().material = null;
        }

        if (Input.GetKeyDown(KeyCode.K)) {
            ObjExporter.MeshToFile(meshFilter,meshRenderer,"tree.obj");
        }
    }

    private void ClearAll()
    {
        ClearStack();
        sentence = "";
        points = null;
        vertices = null;
        uvs = null;
        triangles = null;
        leafs = null;
        leafTriangles = null;
        tangents = null;
        normals = null;
        verticesNum = 0;
        num = 0;
        p = 0;
        v = 0;
        tri = 0;
    }

    private Point PopPoint()
    {
        return stack[top--];
    }

    private void PushPoint()
    {
        stack[++top] = lastPoint;
    }
    
    Matrix4x4 MakeBasis(Vector3 xAxis, Vector3 yAxis, Vector3 zAxis)
    {
        Matrix4x4 mat = Matrix4x4.identity;
        mat.m00 = xAxis.x;
        mat.m10 = xAxis.y;
        mat.m20 = xAxis.z;

        mat.m01 = yAxis.x;
        mat.m11 = yAxis.y;
        mat.m21 = yAxis.z;

        mat.m02 = zAxis.x;
        mat.m12 = zAxis.y;
        mat.m22 = zAxis.z;

        return mat;
    }

    void ClearStack()
    {
        stack = null;
        top = -1;
    }
}